#include <iostream>
#include <string>

#include "data_manager.h"
#include "data_scene.h"
#include "fixed_array.h"
#include "utility.h"

int main(int argc, char const* argv[]) {
  std::filesystem::path dataPath{
      "/home/data/nfsroot/datasets/hj_dataset/fisheye/"
      "2024_04_25_10_45_15_ramp"};
  if (argc > 1) {
    dataPath = std::filesystem::path(argv[1]);
  }

  if (!std::filesystem::exists(dataPath)) {
    std::cout << "[" << autodrive::perception::to_red("Error") << "]"
              << dataPath << " does not exist!!!" << std::endl;
    exit(EXIT_SUCCESS);
  }

  std::cout << "[" << autodrive::perception::to_green("Info") << "]"
            << "Loading Dataset: " << dataPath << std::endl;

  try {
    autodrive::perception::DataScene ds;
    ds.Init(dataPath);

    auto const& dataFrameKeys{ds.DataFrameKeys()};
    if (nullptr != dataFrameKeys) {
      std::shared_ptr<autodrive::perception::DataScene::DataFrame> df_ptr =
          std::make_shared<autodrive::perception::DataScene::DataFrame>();

      autodrive::perception::DataScene::LoadDataFrameSettings ldSettings{
          .lidars{"inno_frl_0_raw"},
          .cameras{"front", "front_fisheye", "left_fisheye", "back_fisheye",
                   "right_fisheye", "back"},
          .radars{}};

      for (auto const& key : *dataFrameKeys) {
        autodrive::perception::Timer t;
        ds.LoadDataFrame(ldSettings, key, *df_ptr);
        std::cout << key << "   " << t.elapsed() / 1000 << "ms" << std::endl;
      }
    } else {
      std::cout << "no dataframe in " << dataPath << std::endl;
    }

    std::cout << "[" << autodrive::perception::to_green("Info") << "]"
              << "----Finish---- " << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "[" << autodrive::perception::to_red("Error") << "]"
              << "run dataset test error, " << e.what() << '\n';
  } catch (...) {
    std::cerr << "[" << autodrive::perception::to_red("Error") << "]"
              << "run dataset test error, unexpected!!!" << '\n';
  }

  try {
    autodrive::perception::DataScene::LoadDataFrameSettings ldSettings{
        .lidars{"inno_frl_0_raw"},
        .cameras{"front", "front_fisheye", "left_fisheye", "back_fisheye",
                 "right_fisheye", "back"},
        .radars{}};

    autodrive::perception::DataManager::DataManagerSettings dsSettings{
        .data_scene_paths{dataPath, dataPath},
        .load_data_frame_settings = ldSettings,
        .cycle = 200};
    auto& dm{autodrive::perception::DataManager::GetInstance()};
    dm.Init(dsSettings);
    dm.Load();
    std::uint64_t currentTimestamp{0};
    while (true) {
      try {
        auto pair{dm.LastestDataFrame()};
        if (pair.first.timestamp != currentTimestamp) {
          currentTimestamp = pair.first.timestamp;
          std::cout << "---------->" << pair.first.timestamp << std::endl;
        }
      } catch (const std::exception& e) {
      }
    }

  } catch (const std::exception& e) {
    std::cerr << "[" << autodrive::perception::to_red("Error") << "]"
              << "run dataset test error, " << e.what() << '\n';
  } catch (...) {
    std::cerr << "[" << autodrive::perception::to_red("Error") << "]"
              << "run dataset test error, unexpected!!!" << '\n';
  }

  exit(EXIT_SUCCESS);
}
